Method and apparatus for estimation of center of gravity using accelerometers

ABSTRACT

A system and associated methodology determine navigation parameters of a vehicle under varying center of gravity position and varying unknown gravitational forces. The system uses high precision accelerometers arranged in a plurality of configurations.

BACKGROUND

Dynamic equations of an aircraft vehicle are derived under theassumption of a known and stationary Center of Gravity (CoG). Variationsin loads result in a change in both of the aircraft vehicle's mass andthe CoG position. The change in the CoG introduces undesirable couplingsin flights dynamics. The variations in loads may be due to fuelconsumption or change in payload of the aircraft vehicle. Knowledge ofthe CoG position is important for determination of the aircraft vehicleattitude, calculation of the various aerodynamics forces and torques,selection of the proper control strategy, and ensuring the aircraftvehicle stability.

An apparatus for tracking the center of gravity of an air vehicle wasdescribed in U.S. Pat. No. 8,260,477B2 entitled “METHOD AND APPARATUSFOR TRACKING CENTER OF GRAVITY OF AIR VEHICLE”, the entire disclosure ofwhich is incorporated herein by reference. The apparatus estimates theangular velocity of the air vehicle from accelerometers measurements.The estimated vehicle angular velocity is then used to calculate both acenter of gravity position and an inertial acceleration with respect tothe vehicle's body frame using a suitable identification technique for alinear-in-parameters system. The estimation of the angular velocity maysuffer from instability due to unstable nonlinear equations used. Theaccelerometers were arranged on rings. The rings are fitted into foreand aft positions of the air vehicles which impose some constraints onthe airframe design.

A passive navigation system (PNS) was described in U.S. Pat. No.6,014,103 entitled “PASSIVE NAVIGATION SYSTEM”, the entire disclosure ofwhich is incorporated herein by reference. The PNS comprises an IMU thatcontains gyroscopes and accelerometers to provide the needed inertialinformation.

An autonomous covert Inertial Navigation System (INS) uniquely suitedfor underwater applications is described in U.S. Pat. No. 5,339,684entitled “GRAVITY AIDED INERTIAL NAVIGATION SYSTEM”, the entiredisclosure of which is incorporated herein by reference. The autonomouscovert INS deals with errors correction where it ensures the Schuler andSidereal errors are bounded. The autonomous covert INS utilizes agradiometer, a gravimeter, and an inertial navigation system (INS) tofacilitate its functionality. It can be used while navigating in varyinggravity fields. However, the autonomous covert INS does not handle theCenter of Gravity (CoG) shift.

A fault tolerant inertial reference system is described in U.S. Pat. No.5,719,764 entitled “FAULT TOLERANT INERTIAL REFERENCE SYSTEM”, theentire disclosure of which is incorporated herein by reference. Thefault tolerant inertial reference system employs two independentinertial reference units. Each unit utilizes global positioning system(GPS) information such as velocity and position. It also providesmaximum level of fault tolerance with a minimum set of redundantinertial sensors where the faulty sensors can be isolated. It alsoprovides means to correct the errors arising from the inertial sensors.However, it does not deal with the varying position of the CoG andvarying gravity.

Therefore, there is a need for a method and system for determining thecenter of gravity position and the position rate of change under theinfluence of varying unknown gravitational force.

The foregoing “background” description is for the purpose of generallypresenting the context of the disclosure. Work of the inventor, to theextent it is described in this background section, as well as aspects ofthe description which may not otherwise qualify as prior art at the timeof filing, are neither expressly or impliedly admitted as prior artagainst the present invention. The foregoing paragraphs have beenprovided by way of general introduction, and are not intended to limitthe scope of the following claims. The described embodiments, togetherwith further advantages, will be best understood by reference to thefollowing detailed description taken in conjunction with theaccompanying drawings.

SUMMARY

The present disclosure relates to a method for determining navigationparameters of a vehicle that receives measurements from at least oneaccelerometer, calculates, using processing circuitry, differentialaccelerometers measurements based on the measurements, calculates anangular acceleration based on the differential accelerometersmeasurements, filters the angular acceleration to obtain an angularvelocity, and calculates the navigation parameters using anidentification technique.

BRIEF DESCRIPTION OF THE DRAWINGS

A more complete appreciation of the disclosure and many of the attendantadvantages thereof will be readily obtained as the same becomes betterunderstood by reference to the following detailed description whenconsidered in connection with the accompanying drawings, wherein:

FIG. 1 is a schematic diagram of sensors for estimating a center ofgravity of a vehicle according to one example;

FIG. 2 is a schematic that shows a pair of rings in a firstconfiguration according to one example;

FIG. 3 is a schematic that shows a ring in a second configurationaccording to one example;

FIG. 4 is a block diagram that shows the calculation of the angularvelocity according to one example;

FIG. 5 is a block diagram that shows the calculation of the attitudeestimation according to one example;

FIG. 6 is a schematic that shows a three-ring configuration according toone example;

FIG. 7 is a flow chart for a QR-decomposition based on a weighted RLS(WRLS) filter according to one example;

FIG. 8 is a block diagram that shows the calculation of inertial dataand acceleration due to gravity according to one example;

FIG. 9 is a block diagram that shows the steps by which theaccelerometers' measurements are used according to one example;

FIG. 10 is a flow chart showing a method for solving the navigationproblem according to one example;

FIG. 11 is an exemplary block diagram of a computer according to oneexample;

FIG. 12 is an exemplary block diagram of a data processing systemaccording to one example; and

FIG. 13 is an exemplary block diagram of a central processing unitaccording to one example.

DETAILED DESCRIPTION

Referring now to the drawings, wherein like reference numerals designateidentical or corresponding parts throughout several views, the followingdescription relates to a system, apparatus, and associated methodologyfor enhanced navigation.

The system of the present disclosure may be used in various militaryapplications and other exploration missions. The system may alsodetermine a gravity tensor of the vehicle, which can be used forcomputer guidance to avoid obstacles. The gravity tensor may also beused for gravity tensor map matching when a reference tensor isavailable. In one embodiment, the system of the present disclosure is apassive Inertial Navigation System (INS).

Inertial navigation units (IMUs) may produce significant errors when thegravity effect is not compensated for. In one embodiment, the system andassociated methodology uses a gravity gradient estimation technique foron-line gravity estimation to compensate for the unknown gravitationalacceleration sensed by motion sensors. The motion sensors may beaccelerometers.

In one embodiment, the system may be a completely passive inertialnavigation system (INS) that can be used in navigating under/water,under/ground, space or air vehicles where other aiding navigationdevices, such as GPS, are not available. This also facilitates its usagein exploration and conducting missions with inaccurately known orcompletely unknown gravity models in a passive mode of operation. Inother embodiments, the system may be used in addition to other aidingnavigation devices.

FIG. 1 is a schematic diagram of sensors for estimating a center ofgravity of a vehicle according to one example. The sensors 106 mayinclude a plurality of accelerometers. The accelerometers may be fixedon the frame of the vehicle 100. The vehicle 100 may be a missile, adrone, an unmanned aerial vehicle (UAV), a spacecraft, a satellite, alander, an aircraft, a watercraft, or the like. The accelerometers maybe aligned with respect to three Euclidean axes fixed on the frame ofthe vehicle. The accelerometers may be positioned using a plurality ofconfigurations. A first configuration 102 requires at least two rings. Asecond configuration 104 has a diamond configuration. Measurements fromthe accelerometers enable finding the angular acceleration of the bodythey are attached to by taking the differential of the measurements asexplained below.

FIG. 2 is a schematic that shows a pair of rings in a firstconfiguration according to one example. The INS includes two or moreIMUs also referred to as rings in the first configuration. In oneexample, as shown in FIG. 2, the INS includes a first IMU 200 and asecond IMU 202. The first IMU 200 and the second IMU 202 may be alignedwith the vehicle's 100 axes (X_(b)Y_(b) Z_(b)). Each ring may include atleast four tri-axial accelerometers. The four tri-axial accelerometersof the first IMU 200 and the second IMU 202 are placed symmetricallyaround a point m at a distance μ. The tri-axial accelerometerscoordinate system (X_(m)Y_(m)Z_(m)) may be also aligned with respect tothe vehicle's 100 axes (X_(b)Y_(b)Z_(b)). In one embodiment, the firstIMU 200 includes four tri-axial accelerometers at positions P1, P2, P3,and P4. The second IMU 202 includes four tri-axial accelerometers atpositions P5, P6, P7, and P8. In FIG. 2, O_(b) represents the CoGposition, O_(n) represents the origin of a inertial coordinate system,R_(I) is a vector from the origin O_(n) to the CoG, L the distancebetween the first and the second IMU 200,202, and R_(v1) and R_(v2)represent the vectors from the CoG to the origin of the first and secondIMU 200, 202 respectively. Each of the first and second IMU 200, 202includes 12 accelerometers. The first and second IMUs 200, 202 furtherinclude processing circuitry. In addition, central processing circuitryis included in the INS. Measurements from the first and the second IMUs200,202 are transmitted to the central processing circuitry.

FIG. 3 is a schematic that shows a ring in the second configurationaccording to one example. The second configuration is the diamondconfiguration in which two linear accelerometers are separated equallyaround a point in three perpendicular directions. The secondconfiguration uses a total of three pairs of tri-axial accelerometersper IMU. FIG. 3 shows a ring 300 in the second configuration. The ring300 includes six tri-axial accelerometers at positions P1, P2, P3, P4,P5 and P6. In one embodiment, the vehicle 100 axis (X_(b)Y_(b)Z_(b)) andthe ring 300 (X_(m)Y_(m)Z_(m)) axis are aligned. A 3D configuration usedin the second configuration allows freedom in positioning the ringswithin the frame of the vehicle 100. The ring includes processingcircuitry. IMUs using ring, in the second configuration, may be used ina decentralized approach while providing improved redundancy andreliability. Thus, the processing circuitry may be embedded in one ormore rings in the second configuration.

The differences between both configurations are the number of linearaccelerometers used, the installation space requirements within theairframe, being self-contained, and sensitivity to the distance betweenredundant rings.

As shown in FIGS. 2 and 3, the accelerometers may be placedsymmetrically around the point m at a distance μ. P_(j) is a tri-axiallinear accelerometer's position, IN is the position of the CoG, O_(n) isthe origin of the inertial coordinate system, R_(I) is the vector frominertial frame origin O_(n) to CoG, and R_(vi) is the vector from theCoG O_(b) to origin of Ring (i), where i=1, 2, 3, . . . etc.

Adopting a flat non-rotating earth model, the acceleration at pointP_(j), shown in FIGS. 2 and 3, may be expressed as:

{right arrow over (A)} _(j) ={right arrow over (A)} _(b)+

+

×({right arrow over (R)} _(vi) +μŝ)+2{right arrow over (Ω)}×

+({right arrow over (Ω)}×({right arrow over (R)} _(vi) +μŝ))−{rightarrow over (g)} _(j)  (1)

where {right arrow over (A)}_(b) is the inertial acceleration ofarbitrary point P measured in a body coordinate system,

is the linear acceleration of the i^(th) ring with respect to the bodycoordinate system, {right arrow over ({dot over (R)})}_(vi) is thelinear velocity of the i^(th) ring with respect to the body coordinatesystem, {right arrow over (R)}_(vi) is the vector from the origin of thebody coordinate system to point m in the i^(th) ring, {right arrow over(Ω)} is the angular velocity of the body system,

is the angular acceleration of the body system, {right arrow over(g)}_(j) is the acceleration due to gravity sensed by the j^(th)accelerometer, ŝ is a unit vector in the appropriate primary direction,and x, denotes the cross product between two vectors.

Taking the differential accelerometers' measurements in each primaryaxis for the ring 300 shown in FIG. 3, the following system of equationscan be obtained:

{right arrow over (A)} ₁ −{right arrow over (A)} ₂=2{right arrow over({dot over (Ω)})}×μî+2{right arrow over (Ω)}×({right arrow over(Q)}×uî)−({right arrow over (g)} ₁ −{right arrow over (g)} ₂)  (2)

{right arrow over (A)} ₃ −{right arrow over (A)} ₄=2{right arrow over({dot over (Ω)})}×μ{circumflex over (k)}+2{right arrow over (Ω)}×({rightarrow over (Q)}×uĵ)−({right arrow over (g)} ₃ −{right arrow over (g)}₄)  (3)

{right arrow over (A)} ₅ −{right arrow over (A)} ₆=2{right arrow over({dot over (Ω)})}×μ{circumflex over (k)}+2{right arrow over (Ω)}×({rightarrow over (Q)}×u{circumflex over (k)})−({right arrow over (g)} ₅−{right arrow over (g)} ₆)  (4)

where, î, ĵ and {circumflex over (k)} are the unit vectors in the ring'sX, Y, and Z axes respectively.

Based on equations (2)-(4), in the second configuration, the gravityacceleration does not affect the angular velocity and accelerationdetermination using the method of the present disclosure when theprecision of the accelerometers is small, for example, when usingMEMS-based accelerometers. However, when the precision of theaccelerometers used is high, such as found in cold-atom interferometry,then the gravity gradient can be measured and the estimation of thegravity acceleration is made possible using the second configuration. Inone embodiment, at least one ring in the second configuration uses highprecision accelerometers. In the first configuration, at least two ringsuse high precision accelerometers.

The gravity vector is a function of the position. So, it may beexpressed as given in T. C. Welker, R. E. Huffman, Jr., and M. Pachter,“Use of Gravity Gradiometry in Precision Inertial Navigation systems”,AIAA Guidance, Navigation, and Control Conference, August (2011)incorporated herein by reference in its entirety:

{right arrow over (g)}=[G _(x)(x(t),y(t),z(t))G _(y)(x(t),y(t),z(t))G_(z)(x(t),y(t),z(t))]^(T)  (5)

The difference between two gravity vectors is considered as the gravitygradient. The symmetric gravity gradient with respect to a body frame isgiven as in A. H. Zorn, “A merging of system technologies:all-accelerometer inertial navigation and gravity gradiometry,” PositionLocation and Navigation Symposium, IEEE, (2002) incorporated herein byreference in its entirety:

$\begin{matrix}\begin{matrix}{\Gamma^{a} = {\nabla g}} \\{\cong {\Delta \; {g/\left( {2\mu} \right)}}} \\{= \begin{bmatrix}\Gamma_{xx} & \Gamma_{xy} & \Gamma_{xz} \\\Gamma_{xy} & \Gamma_{yy} & \Gamma_{yz} \\\Gamma_{xz} & \Gamma_{yz} & \Gamma_{zz}\end{bmatrix}}\end{matrix} & (6)\end{matrix}$

Rearranging the previous equations and taking the appropriate part ofthe gravity gradient according to the axes of concern, then equations(2)-(4) may be rewritten as follows:

$\begin{matrix}{\frac{{\overset{\rightharpoonup}{A}}_{1} - {\overset{\rightharpoonup}{A}}_{2}}{2\mu} = {\left( {\left\lbrack \overset{.}{\Omega} \right\rbrack + \left\lbrack {\Omega \times} \right\rbrack - \Gamma^{a}} \right)\begin{bmatrix}1 \\0 \\0\end{bmatrix}}} & (7) \\{\frac{{\overset{\rightharpoonup}{A}}_{3} - {\overset{\rightharpoonup}{A}}_{4}}{2\mu} = {\left( {\left\lbrack \overset{.}{\Omega} \right\rbrack + \left\lbrack {\Omega \times} \right\rbrack - \Gamma^{a}} \right)\begin{bmatrix}0 \\1 \\0\end{bmatrix}}} & (8) \\{\frac{{\overset{\rightharpoonup}{A}}_{5} - {\overset{\rightharpoonup}{A}}_{6}}{2\mu} = {\left( {\left\lbrack \overset{.}{\Omega} \right\rbrack + \left\lbrack {\Omega \times} \right\rbrack - \Gamma^{a}} \right)\begin{bmatrix}0 \\0 \\1\end{bmatrix}}} & (9)\end{matrix}$

where the cross product was replaced by the multiplication of a skewsymmetric matrix and a vector in the right order. The matrices ([{dotover (Ω)}]), ([Ω×]) are given as follows:

$\begin{matrix}{{\left\lbrack \overset{.}{\Omega} \right\rbrack = \begin{bmatrix}0 & {- {\overset{.}{\Omega}}_{z}} & {\overset{.}{\Omega}}_{y} \\{\overset{.}{\Omega}}_{z} & 0 & {- {\overset{.}{\Omega}}_{x}} \\{- {\overset{.}{\Omega}}_{y}} & {\overset{.}{\Omega}}_{x} & 0\end{bmatrix}},{\left\lbrack {\Omega \times} \right\rbrack = \begin{bmatrix}{{- \Omega_{z}^{2}} - \Omega_{y}^{2}} & {\Omega_{x}\Omega_{y}} & {\Omega_{x}\Omega_{z}} \\{\Omega_{x}\Omega_{y}} & {{- \Omega_{z}^{2}} - \Omega_{x}^{2}} & {\Omega_{z}\Omega_{y}} \\{\Omega_{x}\Omega_{z}} & {\Omega_{z}\Omega_{y}} & {{- \Omega_{y}^{2}} - \Omega_{x}^{2}}\end{bmatrix}}} & (10)\end{matrix}$

By arranging the previous three equations into one-system yields:

$\begin{matrix}{\begin{bmatrix}\frac{{\overset{\rightharpoonup}{A}}_{1} - {\overset{\rightharpoonup}{A}}_{2}}{2\mu} & \frac{{\overset{\rightharpoonup}{A}}_{3} - {\overset{\rightharpoonup}{A}}_{4}}{2\mu} & \frac{{\overset{\rightharpoonup}{A}}_{5} - {\overset{\rightharpoonup}{A}}_{6}}{2\mu}\end{bmatrix} = {\begin{bmatrix}0 & {- {\overset{.}{\Omega}}_{z}} & {\overset{.}{\Omega}}_{y} \\{\overset{.}{\Omega}}_{z} & 0 & {- {\overset{.}{\Omega}}_{x}} \\{- {\overset{.}{\Omega}}_{y}} & {\overset{.}{\Omega}}_{x} & 0\end{bmatrix} + {\quad{\left\lbrack \begin{matrix}{{- \Omega_{z}^{2}} - \Omega_{y}^{2}} & {\Omega_{x}\Omega_{y}} & {\Omega_{x}\Omega_{z}} \\{\Omega_{x}\Omega_{y}} & {{- \Omega_{z}^{2}} - \Omega_{x}^{2}} & {\Omega_{z}\Omega_{y}} \\{\Omega_{x}\Omega_{z}} & {\Omega_{z}\Omega_{y}} & {{- \Omega_{y}^{2}} - \Omega_{x}^{2}}\end{matrix} \right\rbrack - \begin{bmatrix}\Gamma_{xx} & \Gamma_{xy} & \Gamma_{xz} \\\Gamma_{xy} & \Gamma_{yy} & \Gamma_{yz} \\\Gamma_{xz} & \Gamma_{yz} & \Gamma_{zz}\end{bmatrix}}}}} & (11)\end{matrix}$

The same steps can be taken to obtain a set of equations for the firstIMU 200 in the first configuration which may be expressed as follows:

$\begin{matrix}{\begin{bmatrix}\frac{{\sum_{j = 1}^{4}{\overset{\rightharpoonup}{A}}_{1,j}} - {\sum_{j = 1}^{4}{\overset{\rightharpoonup}{A}}_{2,j}}}{2\; \mu} & \frac{{\overset{\rightharpoonup}{A}}_{1,1} - {\overset{\rightharpoonup}{A}}_{1,2}}{2\mu} & \frac{{\overset{\rightharpoonup}{A}}_{1,3} - {\overset{\rightharpoonup}{A}}_{1,4}}{2\mu}\end{bmatrix} = {\quad{\begin{bmatrix}0 & {- {\overset{.}{\Omega}}_{z}} & {\overset{.}{\Omega}}_{y} \\{\overset{.}{\Omega}}_{z} & 0 & {- {\overset{.}{\Omega}}_{x}} \\{- {\overset{.}{\Omega}}_{y}} & {\overset{.}{\Omega}}_{x} & 0\end{bmatrix} + {\quad{\left\lbrack \begin{matrix}{{- \Omega_{z}^{2}} - \Omega_{y}^{2}} & {\Omega_{x}\Omega_{y}} & {\Omega_{x}\Omega_{z}} \\{\Omega_{x}\Omega_{y}} & {{- \Omega_{z}^{2}} - \Omega_{x}^{2}} & {\Omega_{z}\Omega_{y}} \\{\Omega_{x}\Omega_{z}} & {\Omega_{z}\Omega_{y}} & {{- \Omega_{y}^{2}} - \Omega_{x}^{2}}\end{matrix} \right\rbrack - \begin{bmatrix}\Gamma_{xx} & \Gamma_{xy} & \Gamma_{xz} \\\Gamma_{xy} & \Gamma_{yy} & \Gamma_{yz} \\\Gamma_{xz} & \Gamma_{yz} & \Gamma_{zz}\end{bmatrix}}}}}} & (12)\end{matrix}$

where {right arrow over (A)}_(2,j) refers to the j^(th) accelerometer inthe second IMU 202 in the first configuration.

Each of the previous systems of equations given by equations (11) and(12) contains nine equations that need to be solved to determine theelements of the gravity tensor, and both the angular velocity andacceleration. By examining, the systems of equations given by equations(11) and (12) simple linear systems can be obtained, as shown in tables1 and 2, by which the angular acceleration can be obtained directlyusing algebraic calculations. This motivates the usage of anorm-constrained Kalman filter (KF) to retrieve the angular velocitiesand to overcome the drift in the estimation because of the noisyaccelerometers' measurements as described in R. Zanetti, J. Majji, R. H.Bishop, and D. Mortari, “Norm-Constrained Kalman Filtering”, Journal ofGuidance, Control, and Dynamics, (2009) incorporated herein by referencein its entirety.

TABLE 1 Angular acceleration's equations for accelerometers in thesecond configuration State Equations (Second configuration)${\overset{.}{\Omega}}_{x} = {\frac{1}{4\mu}\left( {A_{3z} - A_{4z} - A_{5y} + A_{6y}} \right)}$${\overset{.}{\Omega}}_{y} = {\frac{1}{4\mu}\left( {A_{5x} - A_{6x} - A_{1z} + A_{2z}} \right)}$${\overset{.}{\Omega}}_{z} = {\frac{1}{4\mu}\left( {A_{1y} - A_{2y} - A_{3x} + A_{4x}} \right)}$

TABLE 2 Angular acceleration's equations for accelerometers in the firstconfiguration State Equations (First configuration)${\overset{.}{\Omega}}_{x} = {\frac{1}{4\mu}\left( {A_{1,{1z}} - A_{1,{2z}} - A_{1,{3y}} + A_{1,{4y}}} \right)}$${\overset{.}{\Omega}}_{y} = {{\frac{1}{2\mu}\left( {A_{1,{3x}} - A_{1,{4x}}} \right)} - {\frac{1}{L}\left( {{\sum\limits_{j = 1}^{4}\; A_{1,{jz}}} - {\sum\limits_{j = 1}^{4}\; A_{2,{jz}}}} \right)}}$${\overset{.}{\Omega}}_{z} = {{{- \frac{1}{2\mu}}\left( {A_{1,{1x}} - A_{1,{2x}}} \right)} + {\frac{1}{L}\left( {{\sum\limits_{j = 1}^{4}\; A_{1,{jy}}} - {\sum\limits_{j = 1}^{4}\; A_{2,{jy}}}} \right)}}$

Recalling the fact that the gravity gradient is traceless, it may beshown that the square of the magnitude of the angular velocity for thesecond configuration is given by:

$\begin{matrix}\begin{matrix}{{\overset{\rightharpoonup}{\Omega}}^{2} = {\Omega_{x}^{2} + \Omega_{y}^{2} + \Omega_{z}^{2}}} \\{= {\frac{- 1}{4\mu}\left( {A_{1x} - A_{2x} + A_{3y} - A_{4y} + A_{5z} + A_{6z}} \right)}}\end{matrix} & (13)\end{matrix}$

and for the first configuration is given by:

$\begin{matrix}\begin{matrix}{{\overset{\rightharpoonup}{\Omega}}^{2} = {\Omega_{x}^{2} + \Omega_{y}^{2} + \Omega_{z}^{2}}} \\{= {{\frac{1}{2L}\left( {{\sum\limits_{j = 1}^{4}\; A_{2,{jx}}} - {\sum\limits_{j = 1}^{4}\; A_{1,{jx}}}} \right)} -}} \\{{\frac{1}{4\mu}\left( {A_{1,{1y}} - A_{1,{2y}} + A_{1,{3z}} - A_{1,{4z}}} \right)}}\end{matrix} & (14)\end{matrix}$

This is equal to the square of 2-norm of the angular velocity ({rightarrow over (Ω)}=[Ω_(x), Ω_(y), Ω]^(T)). Equations (13) and (14) provideequality constraints that can be used with norm-constrained KFs toretrieve the angular velocity and at the same time can be used toprovide thresholds for attitude determination. Equations (13) and (14)may not be positive, because of the noise available in theaccelerometers' measurements so they should be tested before applyingthe constraint.

FIG. 4 is a block diagram that shows the calculation of the angularvelocity. The norm measurements module 400 calculates the square of themagnitude of the angular velocity using equation (13). The measurementsmodule 402 calculates the angular acceleration using the state equationsshown in table 1. The output of both modules 400 and 402 are fed to aNorm-constrained Kalman filter 404. Each of the modules described hereinmay be implemented in circuitry that is programmable (e.g.microprocessor-based circuits) or dedicated circuits such as applicationspecific integrated circuits (ASICS) or field programmable gate arrays(FPGAS). The Norm-constrained Kalman filter 404 outputs the angularvelocity. This approach reduces the effect of the estimated angularvelocity on the estimated tiny values, i.e. less than of 10⁻¹⁰, whencalculating the gravity tensor.

After estimating the angular velocity using the norm-constrained Kalmanfilter 404, the gravity tensor can be calculated using the following setof equations for the second configuration:

$\begin{matrix}{{\Gamma_{xx} = {{- \frac{\left( {A_{1x} - A_{2x}} \right)}{2\mu}} - \Omega_{z}^{2} - \Omega_{y}^{2}}}{\Gamma_{yy} = {{- \frac{\left( {A_{3y} - A_{4y}} \right)}{2\mu}} - \Omega_{z}^{2} - \Omega_{x}^{2}}}{\Gamma_{zz} = {{- \frac{\left( {A_{5z} - A_{6z}} \right)}{2\mu}} - \Omega_{z}^{2} - \Omega_{y}^{2}}}{\Gamma_{xy} = {{- \frac{\left( {A_{3x} - A_{4x}} \right) + \left( {A_{1y} - A_{2y}} \right)}{4\mu}} + {\Omega_{x}\Omega_{y}}}}{\Gamma_{xz} = {{- \frac{\left( {A_{5x} - A_{6x}} \right) + \left( {A_{1z} - A_{2z}} \right)}{4\mu}} + {\Omega_{x}\Omega_{z}}}}{\Gamma_{yz} = {{- \frac{\left( {A_{3z} - A_{4z}} \right) + \left( {A_{5y} - A_{6y}} \right)}{4\mu}} + {\Omega_{z}\Omega_{y}}}}} & (15)\end{matrix}$

The gravity tensor traceless property can also be used, where:

trace(Γ^(a))=Γ_(xx)+Γ_(yy)+Γ_(zz)=0.  (16)

A similar approach may be used to obtain the gravity tensor equationsfor the first configuration.

FIG. 5 is a block diagram that shows the calculation of the attitudeestimation according to one example. A norm-constrained KF 506 may beused to solve the attitude problem given by (17).

$\begin{matrix}{\begin{bmatrix}{\overset{.}{q}}_{0} \\{\overset{.}{q}}_{1} \\{\overset{.}{q}}_{2} \\{\overset{.}{q}}_{3}\end{bmatrix} = {0.5*{\begin{bmatrix}0 & {- p} & {- q} & {- r} \\p & 0 & r & {- q} \\q & {- r} & 0 & p \\r & q & {- p} & 0\end{bmatrix}\begin{bmatrix}q_{0} \\q_{1} \\q_{2} \\q_{3}\end{bmatrix}}}} & (17)\end{matrix}$

And it makes use of quaternion norm constraints 502:

∥{right arrow over (q)}∥ ² =q ₀ ² +q ₁ ² +q ₂ ² +q ₃ ²=1  (18)

In addition to equation (18), the magnitude of the angular velocitygiven by equations (13) and (14) is used to build a maneuver detectionthreshold that is used to trigger the attitude update as shown in FIG.5. The Norm-Constrained Kalman Filter 506 is used to retrieve theQuaternion vector which may be used to find a directional cosine matrix(DCM). The angular velocity norm measurements 500 calculate the squareof the magnitude of the angular velocity using equation (13). TheNorm-Constrained Kalman Filter 506 uses the quaternion norm constraint502, the angular velocity norm measurements 500, and the angularvelocity 504 calculated as shown in FIG. 4 to solve the attitudeproblem.

The concept of redundant rings may be utilized to reflect the positionof CoG into equation (1). When two or more rings are included in thesystem, measurements from a faulty ring may be neglected. For example,when the measurements from a ring are out of preset boundaries. Thisapproach helps in increasing the availability and reliability ofmeasurements and estimation of the unknowns and make the configurationless dependent on a particular ring which allows excluding a ring'sresults once it is deemed faulty. When redundant rings are used, acentralized or decentralized approach can be used to fuse themeasurements and estimations of the available rings. Avionic networks,or the like, can be used to connect the rings to each other and to thecentral processing circuitry.

Since the equations are to be derived with respect to the ring's level,it is better to refer to its center instead of referring to individualaccelerometers within the same ring. This approach increases thereliability of measurements within the same ring even in the case of aparticular accelerometer failure. In one embodiment, the health of eachchannel in an accelerometer within the ring may be checked using themethod disclosed in U.S. Pat. No. 8,260,477B2 entitled “METHOD ANDAPPARATUS FOR TRACKING CENTER OF GRAVITY OF AIR VEHICLE”, the entiredisclosure of which is incorporated herein by reference. Once thisapproach is used, then the data processing within the ring is notdependent on its radius (μ). However, the design process of the ring ismore concerned about reducing the sizing effect by choosing its radius(μ) in an optimal fashion that complies with the vehicle's constraintsand the desired measurements' sensitivity.

When all the accelerometers' measurements are correct, i.e. have beenchecked, the acceleration at the center of the (i^(th)) ring may beexpressed as:

$\begin{matrix}{{{\overset{\rightharpoonup}{a}}_{ri} = \frac{\Sigma_{x = 1}^{V}{\overset{\rightharpoonup}{A}}_{x}}{V}},{i = 1},2,{{\ldots \mspace{14mu} N};{V = \left\{ {4,6} \right\}}}} & (19)\end{matrix}$

for the first and second configurations respectively, where N is thenumber of redundant rings.

FIG. 6 is a schematic that shows a three-ring configuration according toone example. FIG. 6 shows three rings 602, 604, 606 located inside arigid body 600. Each of the three rings 602, 604, 606 may use the secondconfiguration. Point 610 represents the CoG of the rigid body 600. 608represents a fixed reference on the rigid body 600. {right arrow over(R)}₁, {right arrow over (R)}₂, {right arrow over (R)}₃ and {right arrowover (R)}_(G) are position vectors between the fixed reference 608, andthe rings 602,604,606 and the CoG 610. {right arrow over (L)}₁₂, {rightarrow over (L)}₂₁, {right arrow over (L)}₂₃ are distance vector betweenthe rings 602, 604, 606. {right arrow over (R)}_(v1), {right arrow over(R)}_(v2), {right arrow over (R)}_(v3) are the position vector betweenthe CoG 610 and the rings 602, 604, 606 respectively. Then, thefollowing equations may be derived

{right arrow over (R)} ₁ −{right arrow over (R)} _(G) ={right arrow over(R)} _(v1)

{right arrow over (R)} ₂ −{right arrow over (R)} _(G) ={right arrow over(R)} _(v2)

{right arrow over (R)} ₃ −{right arrow over (R)} _(G) ={right arrow over(R)} _(v3)  (20)

Substituting equation (20) into equation (1) results in:

{right arrow over (a)} _(ri) −

×{right arrow over (R)} _(i)−{right arrow over (Ω)}×({right arrow over(Ω)}×{right arrow over (R)} _(i))={right arrow over (A)} _(b) −

−

×{right arrow over (R)} _(G)−2{right arrow over (Ω)}×

−{right arrow over (Ω)}×({right arrow over (Ω)}×{right arrow over (R)}_(G))−{right arrow over (g)} _(b)  (21)

where, ({right arrow over (R)}_(i)) is the position of the (i^(th)) Ringwith respect to the fixed reference 608 on the rigid body 600.

Recall that the gravitational gradient is given by equation (6) fromwhich the gravity vector can be retrieved, in the body frame, and may beexpressed as:

$\begin{matrix}{{{{\overset{\rightharpoonup}{g}}_{b}(t)} = {\begin{bmatrix}g_{x} \\g_{y} \\g_{z}\end{bmatrix} = {{{\overset{\rightharpoonup}{g}}_{b\; 0}(t)} + {\int_{0}^{t}{\Gamma^{a}{\overset{\rightharpoonup}{V}}_{b}\ {t}}}}}}{{{\overset{\rightharpoonup}{g}}_{b\; 0}(t)} = {{{DCM}(t)}*{\overset{\rightharpoonup}{g}}_{I\; 0}}}} & (22)\end{matrix}$

where ({right arrow over (V)}_(b)) is the body inertial velocityevaluated in the body frame, ({right arrow over (g)}_(b0)) is thegravity vector given at the initial position in the body frame and({right arrow over (g)}₁₀) is the gravity vector given at the initialposition which is assumed to be known to certain accuracy in theinertial frame and (DCM) is the directional cosine matrix. When ({rightarrow over (V)}_(b)) is small so that it does not violate the assumptionof constant gravitational gradient within a finite number of successivesampling intervals as described in n U.S. Pat. No. 6,014,103 entitled“PASSIVE NAVIGATION SYSTEM”, the entire disclosure of which isincorporated herein by reference. Then the gravity vector may beexpressed as:

$\begin{matrix}{{{\overset{\rightharpoonup}{g}}_{b}(t)} = {\begin{bmatrix}g_{x} \\g_{y} \\g_{z}\end{bmatrix} = {{{\overset{\rightharpoonup}{g}}_{b\; 0}(t)} + {\Gamma^{a}{\overset{\rightharpoonup}{S}(t)}}}}} & (23)\end{matrix}$

where, ({right arrow over (S)}) is the inertial position of the vehiclemeasured in the body frame and its second derivative is the linearinertial acceleration of the vehicle ({right arrow over (A)}_(b))measured at the center of gravity with respect to the body frame. Thegeneral equation is given as follows making use of (22):

$\begin{matrix}{{{\overset{\rightharpoonup}{a}}_{ri} - {\overset{.}{\overset{\rightharpoonup}{\Omega}} \times {\overset{\rightharpoonup}{R}}_{i}} - {\overset{\rightharpoonup}{\Omega} \times \left( {\overset{\rightharpoonup}{\Omega} \times {\overset{\rightharpoonup}{R}}_{i}} \right)} + {\overset{\rightharpoonup}{g}}_{b\; 0}} = {{\overset{\rightharpoonup}{A}}_{b} - {\overset{¨}{\overset{\rightharpoonup}{R}}}_{G} - {\overset{.}{\overset{\rightharpoonup}{\Omega}} \times {\overset{\rightharpoonup}{R}}_{G}} - {2\overset{\rightharpoonup}{\Omega} \times {\overset{.}{\overset{\rightharpoonup}{R}}}_{G}} - {\overset{\rightharpoonup}{\Omega} \times \left( {\overset{\rightharpoonup}{\Omega} \times {\overset{\rightharpoonup}{R}}_{G}} \right)} - {\int_{0}^{t}{\Gamma^{a}{\overset{\rightharpoonup}{V}}_{b}\ {t}}}}} & (24)\end{matrix}$

In the case of constant gravity gradient, equation (24) can be expressedas:

$\begin{matrix}{{{\overset{\rightharpoonup}{a}}_{ri} - {\overset{.}{\overset{\rightharpoonup}{\Omega}} \times {\overset{\rightharpoonup}{R}}_{i}} - {\overset{\rightharpoonup}{\Omega} \times \left( {\overset{\rightharpoonup}{\Omega} \times {\overset{\rightharpoonup}{R}}_{i}} \right)} + {\overset{\rightharpoonup}{g}}_{b\; 0}} = {{\overset{\rightharpoonup}{A}}_{b} - {\overset{¨}{\overset{\rightharpoonup}{R}}}_{G} - {\overset{.}{\overset{\rightharpoonup}{\Omega}} \times {\overset{\rightharpoonup}{R}}_{G}} - {2\overset{\rightharpoonup}{\Omega} \times {\overset{.}{\overset{\rightharpoonup}{R}}}_{G}} - {\overset{\rightharpoonup}{\Omega} \times \left( {\overset{\rightharpoonup}{\Omega} \times {\overset{\rightharpoonup}{R}}_{G}} \right)} - {\Gamma^{a}\overset{\rightharpoonup}{S}}}} & (25)\end{matrix}$

Equation (24) can be discretized to approximate theintegral-differential equation to bring about identifiability. It isclear from equations (24) and (25) that the CoG acceleration (

) cannot be determined, at least using this approach. However, itsvelocity (

) can be determined. Using the skew-symmetric matrix notation instead ofthe cross product, the following models can be obtained:

Model I:

{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([

]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−

−∫₀ ^(t)Γ^(a) {right arrow over (V)} _(b) dt}−2[Ω]

−([

]+[Ω×]){right arrow over (R)} _(G)  (26)

Model II:

{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([

]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−

−Γ^(a) {right arrow over (S)}}−2[Ω]

−([

]+[Ω×]){right arrow over (R)} _(G)  (27)

Model III:

{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([

]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−∫₀ ^(t)Γ^(a) {right arrow over (V)} _(b) dt}−([

]+[Ω×]){right arrow over (R)} _(G)  (28)

Model IV:

{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([

]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−Γ^(a) {right arrow over (S)}}−([

]+[Ω×]){right arrow over (R)} _(G)  (29)

-   -   where:    -   {right arrow over (a)}_(ri): is the acceleration measurement at        the center of the i^(th) ring, given by equation (19),    -   [        ] and [Ω×]: are given by equation (10), and both are known,    -   [Ω]: is the skew matrix of the angular velocity vector, and it        is also known,    -   {right arrow over (g)}_(b0): is the initial gravity vector at        time (t₀), given by equation (22),    -   : is the inertial acceleration of the vehicle measured at CoG in        the body frame, which is to be identified, and it is equivalent        to {right arrow over (A)}_(b).    -   and        : are the acceleration and the velocity of the CoG respectively,        which are to be identified, if appropriate,    -   {right arrow over (R)}_(G): is the position of CoG, which is to        be identified,    -   Γ^(a): is the gravity gradient measured in the body frame given        by equation (6), and it is known,    -   {right arrow over (V)}_(b): is the inertial velocity of the        vehicle measured in the body frame, which is to be identified,        and    -   {right arrow over (S)}: is the inertial position of the vehicle        measured in the body frame, which is to be identified.

In Models I and II, the CoG acceleration (

), when it exists, affects the body inertial acceleration estimation({right arrow over (A)}_(b)=

). In Models III and IV, a simpler approach is taken by considering theCoG to be stationary, i.e.

=

=0. In equations (26)-(29), backward finite-divided-difference formulasmay be used as given by equations (30)-(32) below to obtain a numericalaccuracy equals to 0(h²) where h is the sampling period.

$\begin{matrix}{{\overset{.}{f}\left( x_{i} \right)} \cong \frac{{3\; {f\left( x_{i} \right)}} - {4\; {f\left( x_{i - 1} \right)}} + {f\left( x_{i - 2} \right)}}{2h}} & (30) \\{{\overset{¨}{f}\left( x_{i} \right)} \cong \frac{{2\; {f\left( x_{i} \right)}} - {5\; {f\left( x_{i - 1} \right)}} + {4{f\left( x_{i - 2} \right)}} - {f\left( x_{i - 3} \right)}}{h^{2}}} & (31) \\{{\int_{0}^{t}{\Gamma^{a}{{\overset{\rightharpoonup}{V}}_{b}.\ {t}}}} \cong {\sum\limits_{i = 1}^{N}\; {\frac{h}{2}\left\{ {{\int_{t_{i}}^{t_{i + 1}}{{\Gamma^{a}\left( t_{i} \right)}{\overset{.}{\overset{\rightharpoonup}{S}}\left( t_{i} \right)}}} + {{\Gamma^{a}\left( t_{i - 1} \right)}{\overset{.}{\overset{\rightharpoonup}{S}}\left( t_{i - 1} \right)}}} \right\}}}} & (32)\end{matrix}$

Hence, equation (26) can be given as follows when (

=0):

$\begin{matrix}{{{2h^{2}{m(k)}} + {\frac{h^{2}}{2}{\sum\limits_{i = 1}^{k - 1}\; \left\{ {{3{\Gamma^{a}(i)}{\overset{\rightharpoonup}{S}(i)}} + {\left( {{3{\Gamma^{a}\left( {i - 1} \right)}} - {4{\Gamma^{a}(i)}}} \right){\overset{\rightharpoonup}{S}\left( {i - 1} \right)}} + {\left( {{\Gamma^{a}(i)} - {4{\Gamma^{a}\left( {i - 1} \right)}}} \right){\overset{\rightharpoonup}{S}\left( {i - 2} \right)}} + {{\Gamma^{a}\left( {i - 1} \right)}{\overset{\rightharpoonup}{S}\left( {i - 3} \right)}}} \right\}}}} = {\left\{ {{\left( {{4I} - {\frac{3h^{2}}{2}{\Gamma^{a}(k)}}} \right){\overset{\rightharpoonup}{S}(k)}} - {\left( {{2{h^{2}\left( {{\left\lbrack \overset{.}{\Omega} \right\rbrack (k)} + {\left\lbrack {\Omega \times} \right\rbrack (k)}} \right)}} + {6{h\lbrack\Omega\rbrack}(k)}} \right) {{\overset{\rightharpoonup}{R}}_{G}( k)}}} \right\} - \left\{ {{\left( {{10I} + {\frac{h^{2}}{2}\left( {{3{\Gamma^{a}\left( {k - 1} \right)}} - {4{\Gamma^{a}(k)}}} \right)}} \right){\overset{\rightharpoonup}{S}\left( {k - 1} \right)}} - {\left( {8{h\lbrack\Omega\rbrack}(k)} \right){{\overset{\rightharpoonup}{R}}_{G}\left( {k - 1} \right)}}} \right\} + \left\{ {{\left( {{8I} - {\frac{h^{2}}{2}\left( {{\Gamma^{a}(k)} - {4{\Gamma^{a}\left( {k - 1} \right)}}} \right)}} \right){\overset{\rightharpoonup}{S}\left( {k - 2} \right)}} - {\left( {2{h\lbrack\Omega\rbrack}(k)} \right){{\overset{\rightharpoonup}{R}}_{G}\left( {k - 2} \right)}}} \right\} - \left\{ {\left( {{2I} + {\frac{h^{2}}{2}{\Gamma^{a}\left( {k - 1} \right)}}} \right){\overset{\rightharpoonup}{S}\left( {k - 3} \right)}} \right\}}} & (33)\end{matrix}$

Similarly, equation (27) can be given as follows using 0(h²) and (

=0):

h ² m(k)={(2l−h ²Γ^(a)(k)){right arrow over (S)}(k)−(h ²([{dot over(Ω)}](k)+[Ω×](k))+3h[Ω](k)){right arrow over (R)} _(G)(k)}−{5{rightarrow over (S)}(k−1)−(4h[Ω](k)){right arrow over (R)}_(G)(k−1)}+{4{right arrow over (S)}(k−2)}−(h[Ω](k)){right arrow over(R)} _(G)(k−2))−{{right arrow over (S)}(k−3)}  (34)

In addition, equation (28) may be expressed using 0(h²) as:

$\begin{matrix}{{{2h^{2}{m(k)}} + {\frac{h^{2}}{2}{\sum\limits_{i = 1}^{k - 1}\; \left\{ {{3{\Gamma^{a}(i)}{\overset{\rightharpoonup}{S}(i)}} + {\left( {{3{\Gamma^{a}\left( {i - 1} \right)}} - {4{\Gamma^{a}(i)}}} \right){\overset{\rightharpoonup}{S}\left( {i - 1} \right)}} + {\left( {{\Gamma^{a}(i)} - {4{\Gamma^{a}\left( {i - 1} \right)}}} \right){\overset{\rightharpoonup}{S}\left( {i - 2} \right)}} + {{\Gamma^{a}\left( {i - 1} \right)}{\overset{\rightharpoonup}{S}\left( {i - 3} \right)}}} \right\}}}} = {\left\{ {{\left( {{4I} - {\frac{3h^{2}}{2}{\Gamma^{a}(k)}}} \right){\overset{\rightharpoonup}{S}(k)}} - {\left( {2{h^{2}\left( {{\left\lbrack \overset{.}{\Omega} \right\rbrack (k)} + {\left\lbrack {\Omega \times} \right\rbrack (k)}} \right)}} \right) {{\overset{\rightharpoonup}{R}}_{G}( k)}}} \right\} - \left\{ {\left( {{10I} + {\frac{h^{2}}{2}\left( {{3{\Gamma^{a}\left( {k - 1} \right)}} - {4{\Gamma^{a}(k)}}} \right)}} \right){\overset{\rightharpoonup}{S}\left( {k - 1} \right)}} \right\} + \left\{ {\left( {{8I} - {\frac{h^{2}}{2}\left( {{\Gamma^{a}(k)} - {4{\Gamma^{a}\left( {k - 1} \right)}}} \right)}} \right){\overset{\rightharpoonup}{S}\left( {k - 2} \right)}} \right\} - \left\{ {\left( {{2I} + {\frac{h^{2}}{2}{\Gamma^{a}\left( {k - 1} \right)}}} \right){\overset{\rightharpoonup}{S}\left( {k - 3} \right)}} \right\}}} & (35)\end{matrix}$

Equation (29) can be given as follows using 0(h²):

h ² m(k)={(2l−h ²Γ^(a)(k)){right arrow over (S)}(k)−(h ²[{dot over(Ω)}](k)+[Ω×](k))){right arrow over (R)} _(G)(k)}−{5{right arrow over(S)}(k−1)}+{4{right arrow over (S)}(k−2)}−{{right arrow over(S)}(k−3)}  (36)

Regression forms of the above equations may be used in aQR-Decomposition based weighted recursive least squares (RLS) filer orthe like to retrieve the unknown parameters. The regression form ofequations (33) and (34) may be expressed as:

M(k)=[a ₁ ,a ₂ ,a ₃ ,b ₁ ,b ₂ ,b ₃ ,b ₄ ][{right arrow over (R)}_(G)(k),{right arrow over (R)} _(G)(k−1),{right arrow over (R)}_(G)(k−2),{right arrow over (S)}(k),{right arrow over (S)}(k−1),{rightarrow over (S)}(k−2),{right arrow over (S)}(k−3)]^(T)  (37)

The regression form of equations (35) and (36) may be expressed as:

M(k)=[a ₁ ,b ₁ ,b ₂ ,b ₃ ,b ₄ ][{right arrow over (R)} _(G)(k),{rightarrow over (S)}(k),{right arrow over (S)}(k−1),{right arrow over(S)}(k−2),{right arrow over (S)}(k−3)]^(T)  (38)

FIG. 7 is a flow chart for a QR-decomposition based on a weighted RLS(WRLS) filter according to one example. Equations (37-38) can beimplemented within a QR-D based WRLS scheme as follows: At step S700,the QR-decomposition of the regression matrix (D) to enhance itscondition number is found. In other embodiments, other methods may beused such as Householder, Givens rotations, or the like as would beunderstood by one of ordinary skill in the art. At step S702, theregression form equations (37) and (38) are updated as follows:

{right arrow over (M)}=D{right arrow over (x)}=Q*R*{right arrow over(x)}→Q ^(T) *{right arrow over (M)}=R*{right arrow over (x)}

Q ^(T) *{right arrow over (M)}=R*{right arrow over (x)}→{right arrowover (w)}=R*{right arrow over (x)}  (39)

At step S704, the equation from step S702 are used in a WRLS scheme asfollows:

$\begin{matrix}{{K_{ki} = \frac{P_{{({k - 1})}i}R_{ki}^{T}}{{FF} + {{trace}\left( {R_{ki}P_{{({k - 1})}i}R_{ki}^{T}} \right)}}}{{{\overset{\rightharpoonup}{e}}_{ki}(k)} = {{\overset{\rightharpoonup}{w}(k)} - {R_{ki}\left( {k - 1} \right)}}}{{(k)} = {{\left( {k - 1} \right)} + {K_{ki}{{\overset{\rightharpoonup}{e}}_{ki}(k)}}}}P_{ki} = {\frac{1}{FF}\left( {I - {K_{ki}R_{ki}}} \right)P_{{({k - 1})}i}}} & (40)\end{matrix}$

The following expression helps keeping the covariance matrix positive:

P _(ki)=0.5*(P _(ki) ^(T) +P _(ki))

Where, (i) denotes the ring index, i.e. i=1, 2, etc. . . . and k is theiteration index. At step S706, the processing circuitry may checkwhether the trace of the covariance matrix is larger than a threshold.In response to determining that the trace (Trace(P_(ki))>TH) then

P _(ki)=β*eye(length(Regression Vector))  (41)

at step S708. The threshold value and β may be selected based on trialand error. In one embodiment, the threshold value and β may bedetermined based on past data. In other embodiments, the threshold valueand β may be determined adaptively using the processing circuitry.

Compensation for the varying gravity must also be done before theaccelerometers' measurements are made available to the INSmechanization. In one embodiment, the gravity effect is discretized aswas shown in Models I-IV. Then, these models can be used with theQR-decomposition based WRLS, or the like, to estimate the CoGposition/velocity, the inertial position, velocity and acceleration. Thelatter is corrected by removing the contribution of the gravity tensorfrom the estimated values by adding/subtracting the appropriate amountfrom {right arrow over (S)}(k), {right arrow over (S)}(k−1), {rightarrow over (S)}(k−2), and {right arrow over (S)}(k−3).

FIG. 8 is a block diagram that shows the calculation of inertial dataand gravitational acceleration according to one example. The calculationof the inertial data and the acceleration due to gravity can beestimated as described in A. H. Zorn, “A merging of system technologies:all-accelerometer inertial navigation and gravity gradiometry,” PositionLocation and Navigation Symposium, IEEE, (2002) incorporated herein byreference in its entirety.

FIG. 9 is a block diagram that shows the steps by which theaccelerometers' measurements are used according to one example. FIG. 9shows the flow of measurements and calculated data in the IMU 300 usingthe QR-decomposition based WRLS filter 900. The compensation for gravityand shift from CoG position are done simultaneously. The inertial dataare readily available. In selected embodiments, the inertial data may bere-estimated using a dedicated filter based on Discrete Wiener ProcessAcceleration (DWPA) model.

The system and associated methodology of the present disclosure may beused with a plurality of navigation frames such as Earth centered earthfixed (ECEF) or the like. In other embodiments, the position, velocityand acceleration may be calculated independently of each other. Thesystem and associated methodology may also include GPS measurements orthe like. The equations may be modified to adopt needed navigationframes and components.

FIG. 10 is a flow chart showing a method for solving the navigationproblem according to one example. At step S1000, the accelerometers'measurements are obtained from each of the accelerometers of the IMU.The accelerometer's measurement subjected to a gravity force and locatedaway from the COG of the vehicle may be represented by equation (1). Atstep S1002, the differential outputs of the accelerometers' measurementsare calculated as given in equations (2) and (3). At step S1004, theangular acceleration, using the equations given in table 1 and table 2for the second and the first configuration, may be calculated. At stepS1006, the angular velocities are retrieved using the norm-constrainedKalman filter. The norm-constrained Kalman filter is used because itallows direct estimation of the angular velocities without affecting thegravity tensor as shown in equation (15). The norm-constrained Kalmanfilter allows separating the estimations of both the angular velocityand the gravity tensor components into two steps and the avoiding ofestimation drift through the usage of constraint measurements. Otherfiltering techniques such as a linear Kalman filter may be used as wouldbe understood by one of ordinary skill in the art. At step S1008, theQuaternion vector is estimated using the norm constrained Kalman filterand using equations (17) and (18). At step S1010, the vehicle's attitudeDCM is calculated from the Quaternion vector. Having the attitudeestimated at step S1010, ensures the linear-in-parameter property of theidentification problem as presented in equation (21). Equation (21) withits current format is not identifiable; because of the additive termsappearing in the expression, namely: {right arrow over (A)}_(b),

, and {right arrow over (g)}_(b). In selected embodiments,

can be assumed to be 0. Equations (22) and (23) provides a relationbetween the inertial acceleration and the acceleration due to thegravity based on the position as shown in equation (5). Equations(22-23) are two representatives of the gravity as a function of inertialvelocity and position respectively. Both equations assume that thegravity includes two parts, namely: known and unknown. The known partdepends on the initial value of the gravity just right before the systemis started, and this value may be multiplied at each time instant by theDCM to compensate for the attitude changes. Doing that, the known partcan now be used at the left hand side of equation (21). The unknownpart, which resembles the change occurring in the gravity as the vehicletravels, needs to be identified so that it is kept in the right handside of equation (21).

Equations (21-23) can be used to obtain equations (24-25) from whichfour main models are obtained. At step S1012, the appropriate model isselected. The model is selected based on the application of the systemof the current disclosure. For example, Models I and III can be usedwhen the vehicle is traveling with very high speed which may violate theassumption of constant gravity gradient between two successiveintervals. At step S1014, the discretized and the regression form of themodel are obtained as expressed by equations (33)-(36) and equations(37)-(38) respectively. Using discretization helps in capturing thechanges in the parameters and finding all the inertial parameters at thesame time. At step S1015, the QR-D based WRLS filter is applied to theregression form to retrieve the unknown parameters. Other forms andschemes may be used as well such as using adaptive techniques.

The existence of angular motion (velocity and/or acceleration) isessential for estimating the CoG position and its rate of change. Whenthe angular motion does not exist, then the accelerometers' measurementsare not affected by the CoG shift. Thus, the system of the proposeddisclosure and associated methodology may be used with under/groundvehicles even when the surface is flat/inclined provided that initialattitude information is available.

The system and associated methodology of the present disclosure enablessolving the navigation problem of the vehicle under the simultaneousaction of varying unknown gravity force and center of gravity position.The method calculates navigation parameters such as inertial position,velocity and acceleration, angular velocity and acceleration, theattitude, estimating the position of center of gravity and its rate ofchange, estimating the acceleration due to gravity, and estimating thegravity tensor using only linear accelerometers.

In one embodiment, measurements from the IMUs may be sent via a network1128 to a computer. In other embodiments, the IMUs may includeprocessing circuitry to compute the navigation parameters. The IMU mayinclude a data processing system, as shown in FIG. 12, to create aparticular machine for implementing the above-noted process.

Next, a hardware description of the computer according to exemplaryembodiments is described with reference to FIG. 11. In FIG. 11, thecomputer includes a CPU 1100 which performs the processes describedabove/below. The process data and instructions may be stored in memory1102. These processes and instructions may also be stored on a storagemedium disk 1104 such as a hard drive (HDD) or portable storage mediumor may be stored remotely. Further, the claimed advancements are notlimited by the form of the computer-readable media on which theinstructions of the inventive process are stored. For example, theinstructions may be stored on CDs, DVDs, in FLASH memory, RAM, ROM,PROM, EPROM, EEPROM, hard disk or any other information processingdevice with which the computer communicates, such as a server orcomputer.

Further, the claimed advancements may be provided as a utilityapplication, background daemon, or component of an operating system, orcombination thereof, executing in conjunction with CPU 1100 and anoperating system such as Microsoft Windows 7, UNIX, Solaris, LINUX,Apple MAC-OS and other systems known to those skilled in the art.

The hardware elements in order to achieve the computer may be realizedby various circuitry elements, known to those skilled in the art. Forexample, CPU 1100 may be a Xenon or Core processor from Intel of Americaor an Opteron processor from AMD of America, or may be other processortypes that would be recognized by one of ordinary skill in the art.Alternatively, the CPU 1100 may be implemented on an FPGA, ASIC, PLD orusing discrete logic circuits, as one of ordinary skill in the art wouldrecognize. Further, CPU 1100 may be implemented as multiple processorscooperatively working in parallel to perform the instructions of theinventive processes described above.

The computer in FIG. 11 also includes a network controller 1106, such asan Intel Ethernet PRO network interface card from Intel Corporation ofAmerica, for interfacing with network 1128. As can be appreciated, thenetwork 1128 can be a public network, such as the Internet, or a privatenetwork such as an LAN or WAN network, or any combination thereof andcan also include PSTN or ISDN sub-networks. The network 1128 can also bewired, such as an Ethernet network, or can be wireless such as acellular network including EDGE, 3G and 4G wireless cellular systems.The wireless network can also be WiFi, Bluetooth, or any other wirelessform of communication that is known.

The computer further includes a display controller 1108, such as aNVIDIA GeForce GTX or Quadro graphics adaptor from NVIDIA Corporation ofAmerica for interfacing with display 1110, such as a Hewlett PackardHPL2445w LCD monitor. A general purpose I/O interface 1112 interfaceswith a keyboard and/or mouse 1114 as well as a touch screen panel 1116on or separate from display 1110. General purpose I/O interface alsoconnects to a variety of peripherals 1118 including printers andscanners, such as an OfficeJet or DeskJet from Hewlett Packard.

A sound controller 1120 is also provided in the computer, such as SoundBlaster X-Fi Titanium from Creative, to interface withspeakers/microphone 1122 thereby providing sounds and/or music.

The general purpose storage controller 1124 connects the storage mediumdisk 1104 with communication bus 1126, which may be an ISA, EISA, VESA,PCI, or similar, for interconnecting all of the components of thecomputer. A description of the general features and functionality of thedisplay 1110, keyboard and/or mouse 1114, as well as the displaycontroller 1108, storage controller 1124, network controller 1106, soundcontroller 1120, and general purpose I/O interface 1112 is omittedherein for brevity as these features are known.

The exemplary circuit elements described in the context of the presentdisclosure may be replaced with other elements and structureddifferently than the examples provided herein. Moreover, circuitryconfigured to perform features described herein may be implemented inmultiple circuit units (e.g., chips), or the features may be combined incircuitry on a single chipset, as shown on FIG. 11.

FIG. 12 shows a schematic diagram of a data processing system, accordingto certain embodiments, for computing the navigation parameters. Thedata processing system is an example of a computer in which specificcode or instructions implementing the processes of the illustrativeembodiments may be located to create a particular machine forimplementing the above-noted process.

In FIG. 12, data processing system 1200 employs a hub architectureincluding a north bridge and memory controller hub (NB/MCH) 1225 and asouth bridge and input/output (I/O) controller hub (SB/ICH) 1220. Thecentral processing unit (CPU) 1230 is connected to NB/MCH 1225. TheNB/MCH 1225 also connects to the memory 1245 via a memory bus, andconnects to the graphics processor 1250 via an accelerated graphics port(AGP). The NB/MCH 1225 also connects to the SB/ICH 1220 via an internalbus (e.g., a unified media interface or a direct media interface). TheCPU Processing unit 1230 may contain one or more processors and even maybe implemented using one or more heterogeneous processor systems.

For example, FIG. 13 shows one implementation of CPU 1230. In oneimplementation, the instruction register 1338 retrieves instructionsfrom the fast memory 1340. At least part of these instructions arefetched from the instruction register 1338 by the control logic 1336 andinterpreted according to the instruction set architecture of the CPU1230. Part of the instructions can also be directed to the register1332. In one implementation, the instructions are decoded according to ahardwired method, and in another implementation, the instructions aredecoded according a microprogram that translates instructions into setsof CPU configuration signals that are applied sequentially over multipleclock pulses. After fetching and decoding the instructions, theinstructions are executed using the arithmetic logic unit (ALU) 1334that loads values from the register 1332 and performs logical andmathematical operations on the loaded values according to theinstructions. The results from these operations can be feedback into theregister and/or stored in the fast memory 1340. According to certainimplementations, the instruction set architecture of the CPU 1230 canuse a reduced instruction set architecture, a complex instruction setarchitecture, a vector processor architecture, a very large instructionword architecture. Furthermore, the CPU 1230 can be based on the VonNeuman model or the Harvard model. The CPU 1230 can be a digital signalprocessor, an FPGA, an ASIC, a PLA, a PLD, or a CPLD. Further, the CPU1230 can be an x86 processor by Intel or by AMD; an ARM processor, aPower architecture processor by, e.g., IBM; a SPARC architectureprocessor by Sun Microsystems or by Oracle; or other known CPUarchitecture.

Referring again to FIG. 12, the data processing system 1200 can includethat the SB/ICH 1220 is coupled through a system bus to an I/O Bus, aread only memory (ROM) 1256, universal serial bus (USB) port 1264, aflash binary input/output system (BIOS) 1268, and a graphics controller1258. PCI/PCIe devices can also be coupled to SB/ICH 1220 through a PCIbus 1262.

The PCI devices may include, for example, Ethernet adapters, add-incards, and PC cards for notebook computers. The Hard disk drive 1260 andCD-ROM 1266 can use, for example, an integrated drive electronics (IDE)or serial advanced technology attachment (SATA) interface. In oneimplementation, the I/O bus can include a super I/O (SIO) device.

Further, the hard disk drive (HDD) 1260 and optical drive 1266 can alsobe coupled to the SB/ICH 1220 through a system bus. In oneimplementation, a keyboard 1270, a mouse 1272, a parallel port 1278, anda serial port 1276 can be connected to the system bust through the I/Obus. Other peripherals and devices that can be connected to the SB/ICH1220 using a mass storage controller such as SATA or PATA, an Ethernetport, an ISA bus, a LPC bridge, SMBus, a DMA controller, and an AudioCodec.

Moreover, the present disclosure is not limited to the specific circuitelements described herein, nor is the present disclosure limited to thespecific sizing and classification of these elements. For example, theskilled artisan will appreciate that the circuitry described herein maybe adapted based on changes on battery sizing and chemistry, or based onthe requirements of the intended back-up load to be powered.

The hardware description above, exemplified by any one of the structureexamples shown in FIGS. 11 and 12 constitutes or includes specializedcorresponding structure that is programmed or configured to perform thealgorithms shown in FIGS. 7 and 10. For example, the algorithms shown inFIGS. 7 and 10 may be completely performed by the circuitry included inthe single device shown in FIG. 11 or the chipset as shown in FIG. 12.

A system which includes the features in the foregoing descriptionprovides numerous advantages to users. In particular, the vehicleinertial measurements and gradiometer functionalities are combined in anAll-accelerometer IMU comprising only linear accelerometers with highprecision. In addition, the present disclosure has the advantage ofminimizing computation requirements and increasing processing speed byusing a one ring configuration, in selected embodiments. In addition,the present disclosure provides an improvement to the technical field bycalculating the navigation parameters under the simultaneous varying ofthe center of gravity position and unknown gravitational force. Thesystem therefore controls navigation of the vehicle based on thenavigation parameters in a more efficient and stable manner. In oneembodiment, the system controls the vehicle's orientation (attitude)about the vehicle COG to ensure the aircraft vehicle stability.

Obviously, numerous modifications and variations are possible in lightof the above teachings. It is therefore to be understood that within thescope of the appended claims, the invention may be practiced otherwisethan as specifically described herein.

Thus, the foregoing discussion discloses and describes merely exemplaryembodiments of the present invention. As will be understood by thoseskilled in the art, the present invention may be embodied in otherspecific forms without departing from the spirit or essentialcharacteristics thereof. Accordingly, the disclosure of the presentinvention is intended to be illustrative, but not limiting of the scopeof the invention, as well as other claims. The disclosure, including anyreadily discernible variants of the teachings herein, define, in part,the scope of the foregoing claim terminology such that no inventivesubject matter is dedicated to the public.

1. A method for determining navigation parameters of a vehicle undervarying center of gravity position and unknown varying gravitationalforce, the method comprising: receiving measurements fromaccelerometers; calculating, using processing circuitry, differentialaccelerometers measurements; calculating, using the processingcircuitry, an angular acceleration; filtering, using the processingcircuitry, the angular acceleration to obtain an angular velocity; andcalculating, using the processing circuitry, the navigation parametersusing an identification technique.
 2. The method of claim 1, wherein thenavigation parameters includes at least one of an inertial position, avelocity, an acceleration, an angular velocity, an angular acceleration,attitude, a position of a center of gravity and its rate of change,gravity tensor, and a gravitational acceleration.
 3. The method of claim2, wherein the angular velocity estimation is isolated from gravitytensor estimation via a constrained filtering technique.
 4. The methodof claim 2, wherein calculating the gravity tensor includes applying:$\Gamma_{xx} = {{- \frac{\left( {A_{1x} - A_{2x}} \right)}{2µ}} - \Omega_{z}^{2} - \Omega_{y}^{2}}$$\Gamma_{yy} = {{- \frac{\left( {A_{3y} - A_{4y}} \right)}{2µ}} - \Omega_{z}^{2} - \Omega_{x}^{2}}$$\Gamma_{zz} = {{- \frac{\left( {A_{5z} - A_{6z}} \right)}{2µ}} - \Omega_{x}^{2} - \Omega_{y}^{2}}$$\Gamma_{xy} = {{- \frac{\left( {A_{3x} - A_{4x}} \right) + \left( {A_{1y} - A_{2y}} \right)}{4µ}} + {\Omega_{x}\Omega_{y}}}$$\Gamma_{xz} = {{- \frac{\left( {A_{5x} - A_{6x}} \right) + \left( {A_{1z} - A_{2z}} \right)}{4µ}} + {\Omega_{x}\Omega_{z}}}$$\Gamma_{yz} = {{- \frac{\left( {A_{3z} - A_{4z}} \right) + \left( {A_{5y} - A_{6y}} \right)}{4µ}} + {\Omega_{z}\Omega_{y}}}$where {right arrow over (A)}_(j) refers to the jth accelerometermeasurement, μ is a distance between the accelerometers and a pointabout which the accelerometers are symmetrically disposed, and Ω is theangular velocity.
 5. The method of claim 2, wherein the calculating ofthe navigation parameters based on the identification technique uses oneor more of the following models{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([{dot over(Ω)}]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−

−∫₀ ^(t)Γ^(a) {right arrow over (V)} _(b) dt}−2[Ω]

−([{dot over (Ω)}]+[Ω×]){right arrow over (R)} _(G),{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([{dot over(Ω)}]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−

−Γ^(a) {right arrow over (S)}}−2[Ω]

−([{dot over (Ω)}]+[Ω×]){right arrow over (R)} _(G),{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([{dot over(Ω)}]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−∫₀ ^(t)Γ^(a) {right arrow over (V)} _(b) dt}−([{dot over(Ω)}]+[Ω×]){right arrow over (R)} _(G), and{right arrow over (m)} _(i)(k)={right arrow over (a)} _(ri)−([{dot over(Ω)}]+[Ω×]){right arrow over (R)} _(i) +{right arrow over (g)} _(b0)={

−Γ^(a) {right arrow over (S)}}−([{dot over (Ω)}]+[Ω×]){right arrow over(R)} _(G) where {right arrow over (a)}_(ri): is an accelerationmeasurement at the center of the i^(th) ring, [Ω]: is a skew matrix ofan angular velocity vector, {right arrow over (g)}_(b0): is an initialgravity vector at time (t₀), {right arrow over (A)}_(b): is an inertialacceleration of the vehicle measured at a center of gravity of a bodyframe of the vehicle,

and

: are the acceleration and the velocity of the center of gravityrespectively {right arrow over (R)}_(G): is the position of the centerof gravity, Γ^(a): is a gravity gradient measured in the body frame,{right arrow over (V)}_(b): is an inertial velocity of the vehicle,{right arrow over (S)}: is an inertial position of the vehicle, [{dotover (Ω)}] is expressed as${\left\lbrack \overset{.}{\Omega} \right\rbrack = \begin{bmatrix}0 & {- {\overset{.}{\Omega}}_{z}} & {\overset{.}{\Omega}}_{y} \\{\overset{.}{\Omega}}_{z} & 0 & {- {\overset{.}{\Omega}}_{x}} \\{- {\overset{.}{\Omega}}_{y}} & {\overset{.}{\Omega}}_{x} & 0\end{bmatrix}},$ and [Ω×] is expressed as$\left\lbrack {\Omega \times} \right\rbrack = {\begin{bmatrix}{{- \Omega_{z}^{2}} - \Omega_{y}^{2}} & {\Omega_{x}\Omega_{y}} & {\Omega_{x}\Omega_{z}} \\{\Omega_{x}\Omega_{y}} & {{- \Omega_{z}^{2}} - \Omega_{x}^{2}} & {\Omega_{z}\Omega_{y}} \\{\Omega_{x}\Omega_{z}} & {\Omega_{z}\Omega_{y}} & {{- \Omega_{y}^{2}} - \Omega_{x}^{2}}\end{bmatrix}.}$
 6. The method of claim 2, wherein calculating thegravitational acceleration includes applying:{right arrow over (g)} ₁(t)=∫₀ ^(t)DCM^(T)Γ^(a) V _(b) dt where the DCMis the directional cosine matrix, Γ^(a) is a gravity gradient, and{right arrow over (V)}_(b) is an inertial velocity of the vehiclemeasured in the body frame of the vehicle.
 7. The method of claim 2,wherein the accelerometers are arranged in one or more distributeddiamond configurations.
 8. The method of claim 7, wherein the processingcircuitry is embedded in the one or more distributed diamondconfigurations.
 9. The method of claim 2, wherein the calculation of thenavigation parameters is fully operated in a passive navigation system.10. The method of claim 2, wherein the navigation parameters arerepresented using different navigation frames.
 11. The method of claim2, wherein the calculating of the navigation parameters is operated incombination with aided navigation systems.
 12. The method of claim 1,wherein the vehicle is one of an air vehicle, an underwater vehicle, anunderground vehicle, or a space vehicle.
 13. The method of claim 1,further comprising: computing a gravity tensor of the vehicle; andsending the gravity tensor to a host application to provide computerguidance to avoid obstacles.
 14. The method of claim 1, wherein theaccelerometers are arranged in two or more rings.
 15. The method ofclaim 14, wherein calculating the angular acceleration includesapplying:${\overset{.}{\Omega}}_{x} = {\frac{1}{4µ}\left( {A_{1,{1z}} - A_{1,{2z}} - A_{1,{3y}} + A_{1,{4y}}} \right)}$${\overset{.}{\Omega}}_{y} = {{\frac{1}{2µ}\left( {A_{1,{3x}} - A_{1,{4x}}} \right)} - {\frac{1}{L}\left( {{\sum\limits_{j = 1}^{4}\; A_{1,{jz}}} - {\sum\limits_{j = 1}^{4}\; A_{2,{jz}}}} \right)}}$${\overset{.}{\Omega}}_{z} = {{{- \frac{1}{2µ}}\left( {A_{1,{1x}} - A_{1,{2x}}} \right)} + {\frac{1}{L}\left( {{\sum\limits_{j = 1}^{4}\; A_{1,{jy}}} - {\sum\limits_{j = 1}^{4}\; A_{2,{jy}}}} \right)}}$where {right arrow over (A)}_(i,j) refers to the jth accelerometermeasurement in the ith ring, μ is a distance between the accelerometersand a point about which the accelerometers are symmetrically disposed,{dot over (Ω)} is the angular acceleration and L is a fixed distancebetween the rings.
 16. The method of claim 1, wherein the accelerometersare arranged in a diamond configuration.
 17. The method of claim 16,wherein calculating the angular acceleration includes applying:${\overset{.}{\Omega}}_{x} = {\frac{1}{4µ}\left( {A_{3z} - A_{4z} - A_{5y} + A_{6y}} \right)}$${\overset{.}{\Omega}}_{y} = {\frac{1}{4µ}\left( {A_{5x} - A_{6x} - A_{1z} + A_{2z}} \right)}$${\overset{.}{\Omega}}_{z} = {\frac{1}{4µ}\left( {A_{1y} - A_{2y} - A_{3x} + A_{4x}} \right)}$where {right arrow over (A)}_(j) refers to the jth accelerometermeasurement, {dot over (Ω)} is the angular acceleration, and μ is adistance between the accelerometers and a point about which theaccelerometers are symmetrically disposed.
 18. The method of claim 1,wherein the filtering is via a norm constrained Kalman filter.
 19. Asystem for determining navigation parameters of a vehicle, the systemcomprising: processing circuitry configured to receive measurements fromaccelerometers, calculate differential accelerometers measurements basedon the measurements, calculate an angular acceleration based on thedifferential accelerometers measurements, filter the angularacceleration to obtain an angular velocity, and calculate the navigationparameters using an identification technique.
 20. An inertial navigationunit comprising: six or more accelerometers; and processing circuitryconfigured to receive measurements from the six or more accelerometers,calculate differential accelerometers measurements based on themeasurements, calculate an angular acceleration based on thedifferential accelerometers measurements, filter the angularacceleration to obtain an angular velocity, and calculate the navigationparameters using an identification technique.